import configparser

import numpy as np


class Config:
    def __init__(self):
        self.cfg = configparser.ConfigParser()

    def read(self, filename):
        self.cfg.read(filename, encoding="utf-8")
        if not (self.cfg.sections().__contains__("IMU_PARA") and
                self.cfg.sections().__contains__("INSTALL_MSG")):
            print("file does not contains IMU_PARA section!!")
            exit(0xffff)

    def lb_gnss(self):
        """
        杆臂INSTALL_MSG
        :return: np.array m
        """
        return np.array(np.double(self.cfg.get("INSTALL_MSG", "lb_gnss").split(",")))

    def pos_std(self):
        """
        位置标准差 m
        :return:
        """
        return np.array(np.double(self.cfg.get("IMU_PARA", "pos_std").split(",")))

    def vel_std(self):
        """
        速度标准差 m/s
        :return:
        """

        return np.array(np.double(self.cfg.get("IMU_PARA", "vel_std").split(",")))

    def atti_std(self):
        """
        姿态标准差 rad
        :return:
        """
        return np.array(np.double(self.cfg.get("IMU_PARA", "atti_std").split(","))) * np.pi / 180.0

    def gb_std(self):
        """
        陀螺零偏标准差 rad/s
        :return: deg/h -> rad/s
        """
        return np.double(self.cfg.get("IMU_PARA", "gb_std").split(",")) * np.pi / 180.0 / 3600.0

    def ab_std(self):
        """
        加速度零偏标准差 m/s^2
        :return:mCal -> m/s^2
        """
        return np.double(self.cfg.get("IMU_PARA", "ab_std").split(",")) * 1e-5

    def gs_std(self):
        """
        陀螺比例因子标准差ppm
        :return:
        """
        return np.double(self.cfg.get("IMU_PARA", "gs_std").split(",")) * 1e-6

    def as_std(self):
        """
        加速度计比例因子标准差 ppm
        :return:
        """
        return np.double(self.cfg.get("IMU_PARA", "as_std").split(",")) * 1e-6

    def at_corr(self):
        """
        相关时间 s
        :return:
        """
        return np.double(self.cfg.get("IMU_PARA", "at_corr")) * 3600.0

    def gt_corr(self):
        """
        相关时间 s
        :return:
        """
        return np.double(self.cfg.get("IMU_PARA", "gt_corr")) * 3600.0

    def gb_init(self):
        """
        初始陀螺仪零偏 deg/h
        :return:
        """
        return np.array(np.double(self.cfg.get("IMU_PARA", "gb_ini").split(','))) \
               * np.pi / 180.0 / 3600

    def ab_init(self):
        """
        初始加速度零偏 mGal
        :return:
        """
        return np.array(np.double(self.cfg.get("IMU_PARA", "ab_ini").split(','))) * 1e-5

    def as_init(self):
        """
        初始加速度比例因子误差 ppm
        :return:
        """
        return np.array(np.double(self.cfg.get("IMU_PARA", "as_ini").split(','))) * 1e-6

    def gs_init(self):
        """
        初始陀螺 比例因子误差 ppm
        :return:
        """
        return np.array(np.double(self.cfg.get("IMU_PARA", "gs_ini").split(','))) * 1e-6

    def arw(self):
        """
        ARW
        :return:
        """
        return np.double(self.cfg.get("IMU_PARA", "arw")) * np.pi / 180.0 / 60.0

    def vrw(self):
        """
        VRW
        :return:
        """
        return np.double(self.cfg.get("IMU_PARA", "vrw")) / 60.0

    def lb_odo(self):
        """
        里程计杆臂 m
        :return:
        """
        return np.array(np.double(self.cfg.get("INSTALL_MSG", "lb_odo").split(',')))

    def ins_angle(self):
        """
        INS安装角rad
        :return:
        """
        return np.array(np.double(self.cfg.get("INSTALL_MSG", "ins_angle").split(',')) * np.pi / 180.0)

    def show(self):
        l = self.lb_gnss()
        l1 = self.lb_odo()
        print("****************************************************************")
        print("IMU参数")
        print("位置误差:[%f,%f,%f]" % (self.pos_std()[0], self.pos_std()[1], self.pos_std()[2]))
        print("速度误差:[%f,%f,%f]" % (self.vel_std()[0], self.vel_std()[1], self.vel_std()[2]))
        print("gb:%f deg/h" % (self.gb_std() * 180 / np.pi * 3600))
        print("ab:%f mGal" % (self.ab_std() * 1.0e5))
        print("gs:%f ppm" % (self.gs_std() * 1e6))
        print("as:%f ppm" % (self.as_std() * 1e6))
        print("ARW:%f deg/sqrt(h)" % (self.arw() * 180 / np.pi * 60))
        print("VRW:%f m/s/sqrt(h)" % (self.vrw() * 60))
        print("相关时间:%f h" % (self.at_corr() / 3600))
        print("安装信息")
        print("天线杆臂:[%f,%f,%f]" % (l[0], l[1], l[2]))
        print("IMU安装角:[%f,%f,%f]" % (
            self.ins_angle()[0] * 180 / np.pi, self.ins_angle()[1] * 180 / np.pi, self.ins_angle()[2] * 180 / np.pi))
        print("里程计杆臂:[%f,%f,%f]" % (l1[0], l1[1], l1[2]))
        print("****************************************************************")

    def __str__(self):
        l = self.lb_gnss()
        l1 = self.lb_odo()
        info = ""
        info += "****************************************************************\n"
        info += "IMU参数\n"
        info += ("位置误差:[%f,%f,%f]\n" % (self.pos_std()[0], self.pos_std()[1],self. pos_std()[2]))
        info += ("速度误差:[%f,%f,%f]\n" % (self.vel_std()[0],self. vel_std()[1], self.vel_std()[2]))
        info += ("gb:%f deg/h\n" % (self.gb_std() * 180 / np.pi * 3600))
        info += ("ab:%f mGal\n" % (self.ab_std() * 1.0e5))
        info += ("gs:%f ppm\n" % (self.gs_std() * 1e6))
        info += ("as:%f ppm\n" % (self.as_std() * 1e6))
        info += ("ARW:%f deg/sqrt(h)\n" % (self.arw() * 180 / np.pi * 60))
        info += ("VRW:%f m/s/sqrt(h)\n" % (self.vrw() * 60))
        info += ("相关时间:%f h\n" % (self.at_corr() / 3600))
        info += '安装信息\n'
        info += ("天线杆臂:[%f,%f,%f]\n" % (l[0], l[1], l[2]))
        info += ("IMU安装角:[%f,%f,%f]\n" % (
            self.ins_angle()[0] * 180 / np.pi, self.ins_angle()[1] * 180 / np.pi,self. ins_angle()[2] * 180 / np.pi))
        info += ("里程计杆臂:[%f,%f,%f]\n" % (l1[0], l1[1], l1[2]))
        info += "****************************************************************"
        return info


def debug():
    cfg = Config()

    cfg.read("imu_m39_parameter.cfg")
    cfg.show()


if __name__ == '__main__':
    debug()
